/*倒立摆控制与仿真*/
#include "pendulum.hpp"
#include "controller.hpp"
#include "graphics.h"

#define SCENE_HALF_WIDTH  640  // 画面宽度/2
#define SCENE_HALF_HEIGHT  220  // 画面高度/2
#define SCENE_WIDTH  (SCENE_HALF_WIDTH*2)  // 画面宽度
#define SCENE_HEIGHT  (SCENE_HALF_HEIGHT*2)  // 画面高度
#define CAR_HALF_WIDTH  20  // 小车宽度/2
#define CAR_HALF_HEIGHT  10  // 小车高度/2
#define CAR_ALTITUDE  200  // 小车底部与画面底部的距离
#define ROD_LENGTH  180  // 摆杆长度
#define CAR_INITIAL_X  0  // 小车起始位置(相对于中点)


struct PendulumState{
    PendulumState() {};
    PendulumState(double x, double theta1, double theta2): x(x), theta1(theta1), theta2(theta2) {}
    double x, theta1, theta2;
};

class Pendulum_Simulator
{
public:
    Pendulum_Simulator() {
        // double lqrK[5] = {-1624.451594031069,  -510.51483322911156,  1772.6649767792776,  797.2069266426854,  -10.000000000305445};
        double lqrK[5] = {0,0,0,0,0};
        mlqr = new LQR_Controller(&sim1, lqrK, 5);
        mpendulum = new InversePendulumTwo(&sim1);
        SMConstant(minu, &sim1);
        SMOutput(moutu, &sim1);
        SMOutput(mouttheta1, &sim1);
        SMOutput(mouttheta2, &sim1);
        SMOutput(moutx, &sim1);
        sim1.connect(minu, mlqr, 5);
        sim1.connect(mpendulum, 0, mlqr, 0);
        sim1.connect(mpendulum, 1, mlqr, 1);
        sim1.connect(mpendulum, 2, mlqr, 2);
        sim1.connect(mpendulum, 3, mlqr, 3);
        sim1.connect(mpendulum, 4, mlqr, 4);
        sim1.connect(mlqr, mpendulum);
        sim1.connect(mpendulum, 0, mouttheta1);
        sim1.connect(mpendulum, 2, mouttheta2);
        sim1.connect(mpendulum, 5, moutx);
        sim1.connect(mlqr, moutu);
        // sim1.Set_EnablePrint(false);
        mouttheta1->Set_EnablePrint(false);
        mouttheta2->Set_EnablePrint(false);
        moutx->Set_EnablePrint(false);
        // minu->Set_OutValue(100);
        sim1.Initialize();
        _simstep = sim1.Get_SimStep();
    };
    PendulumState Simulate(double time) {
        int stepcnt = (int)(time/_simstep/1000.0+0.5);
        for (int i=0; i<stepcnt; ++i)
            sim1.Simulate_OneStep();
        return PendulumState(moutx->Get_OutValue(), mouttheta1->Get_OutValue(), mouttheta2->Get_OutValue());
    }
    void Reset(void){
        sim1.Simulation_Reset();
    }
private:
    Simulator sim1;
    LQR_Controller *mlqr=nullptr;  // LQR控制器
    InversePendulumTwo *mpendulum=nullptr;  // 倒立摆系统
    MConstant *minu=nullptr;
    MOutput *moutu=nullptr;
    MOutput *mouttheta1=nullptr;
    MOutput *mouttheta2=nullptr;
    MOutput *moutx=nullptr;
    double _simstep;
};

int Draw_Pendulum(PendulumState car)
{
   int cx = int(car.x*500+0.5) + SCENE_HALF_WIDTH + CAR_INITIAL_X;
   int cy = SCENE_HEIGHT - CAR_ALTITUDE - CAR_HALF_HEIGHT;
   bar(cx-CAR_HALF_WIDTH, cy-CAR_HALF_HEIGHT, cx+CAR_HALF_WIDTH, cy+CAR_HALF_HEIGHT);
   cy -= CAR_HALF_HEIGHT;
   int bx, by;
   bx = cx - int(ROD_LENGTH*0.5 * sin(car.theta1) + 0.5);
   by = cy - int(ROD_LENGTH*0.5 * cos(car.theta1) + 0.5);
   line(bx - CAR_HALF_WIDTH, by, cx - CAR_HALF_WIDTH, cy);
   bx = cx - int(ROD_LENGTH * sin(car.theta2) + 0.5);
   by = cy - int(ROD_LENGTH * cos(car.theta2) + 0.5);
   line(bx + CAR_HALF_WIDTH, by, cx + CAR_HALF_WIDTH, cy);
   if ((cx<-CAR_HALF_WIDTH) || (cx>SCENE_WIDTH+CAR_HALF_WIDTH)) return 1;
   return 0;
}

int main()
{
    Pendulum_Simulator psim1;
    PendulumState car1(0, 0, 0);
    double msdelays = 5;
    double totaltime = 0;
    initgraph(SCENE_WIDTH, SCENE_HEIGHT);
    setbkcolor(0xFFFFFF);
    setcolor(0);
    setfillcolor(0);
    for (; is_run(); delay_ms(msdelays)) {
        car1 = psim1.Simulate(msdelays);
        cleardevice();
        if (Draw_Pendulum(car1))
            psim1.Reset();
        else if (totaltime>8000) {
            psim1.Reset();
            totaltime = 0;
        }
        totaltime += msdelays;
    }
    return 0;
}
